#include "stdafx.h"
#include <iostream>

void Robotic_Math_Test()
{
	cout<< "Robotic_Math_Test start."<< endl;

	{
		for(int i=0;i<100;i++)
		{
			double joint = 0;
			Random(joint);
			Matrix x = Rotation3::EulerZYX(joint,0,0);
			Matrix xx = Rotation3::EulerZYX(-joint,0,0);
			Matrix xxx = x * xx;
			if(xxx != Rotation3::Identity())
				cout<< "Matrix EulerZYX failed."<< endl;
		}
	}

	{
		for(int i=0;i<100;i++)
		{
			double joint = 0;
			Random(joint);
			Matrix x = Rotation3::EulerZYX(joint,0,0);
			Matrix xx = Rotation3::EulerZYX(-joint,0,0);
			Matrix xxx = x.Inverse();
			if(xxx != xx)
			{
				cout << "Matrix Inverse failed."<< endl;
				cout << "xx"<< endl;
				cout << xx(0,0) << " "<< xx(0,1) << " "<< xx(0,2) << endl;
				cout << xx(1,0) << " "<< xx(1,1) << " "<< xx(1,2) << endl;
				cout << xx(2,0) << " "<< xx(2,1) << " "<< xx(2,2) << endl;
				cout << "xxx"<< endl;
				cout << xxx(0,0) << " "<< xxx(0,1) << " "<< xxx(0,2) << endl;
				cout << xxx(1,0) << " "<< xxx(1,1) << " "<< xxx(1,2) << endl;
				cout << xxx(2,0) << " "<< xxx(2,1) << " "<< xxx(2,2) << endl;

			}
		}
	}


	{
		Matrix x = Rotation3::EulerZYX(PI,0,3);
		Matrix xx = x.Inverse();
		Matrix xxx = x * xx;
		if(xxx != Rotation3::Identity())
			cout<< "Matrix Inverse failed."<< endl;
	}


	cout<< "Robotic_Math_Test end."<< endl << endl;
}